﻿using System.Globalization;

namespace Om.Ptz
{
    public static class Helper
    {
        /// <summary>
        /// 解析云台水平/垂直位置返回指令
        /// </summary>
        /// <param name="hexCommand">十六进制指令字符串（例如："FF 01 00 59 00 64 BE"）</param>
        /// <returns>解析后的角度值（单位：度）</returns>
        /// <exception cref="ArgumentException">当输入格式无效时抛出异常</exception>
        public static double ParseAngle(string hexCommand)
        {
            hexCommand = hexCommand.ToUpper();
            // 参数校验
            if (string.IsNullOrWhiteSpace(hexCommand))
                throw new ArgumentException("输入指令不能为空");

            string[] parts = hexCommand.Trim().Split(
                new[] { ' ' },
                StringSplitOptions.RemoveEmptyEntries
            );

            // 验证基础格式
            if (parts.Length < 7)
                throw new ArgumentException("指令长度不足");

            // 验证指令头
            // [3]=59 水平；[3]=5B 垂直
            if ((parts[3] != "59" && parts[3] != "5B"))
                throw new ArgumentException("无效的指令头");

            // 解析 PMSB 和 PLSB
            if (!TryParseHexByte(parts[4], out byte pmsb))
                throw new ArgumentException($"无效的值: {parts[4]}");

            if (!TryParseHexByte(parts[5], out byte plsb))
                throw new ArgumentException($"无效的值: {parts[5]}");

            // 计算原始数据
            int pdata = (pmsb << 8) | plsb; // 等价于 pmsb*256 + plsb

            // 垂直
            if (parts[3] == "5B")
            {
                pdata = pdata >= 18000 ? 36000 - pdata : -pdata;
            }

            // 计算最终角度
            return pdata / 100.0;
        }
        /// <summary>
        /// 解析云台水平/垂直位置返回指令
        /// </summary>
        /// <param name="hexBytes">十六进制数组（例如："FF 01 00 59 00 64 BE"）</param>
        /// <returns>解析后的角度值（单位：度）</returns>
        /// <exception cref="ArgumentException">当输入格式无效时抛出异常</exception>
        public static double ParseAngle(byte[] hexBytes)
        {
            var parts = hexBytes;
            // 验证基础格式
            if (parts.Length < 7)
                throw new ArgumentException("指令长度不足");

            // 验证指令头
            // [3]=59 水平；[3]=5B 垂直
            if ((parts[3] != 0x59 && parts[3] != 0x5B))
                throw new ArgumentException("无效的指令头");

            // 解析 PMSB 和 PLSB
            var pmsb = parts[4];
            var plsb = parts[5];
            // 计算原始数据
            int pdata = (pmsb << 8) | plsb; // 等价于 pmsb*256 + plsb

            // 垂直
            if (parts[3] == 0x5B)
            {
                pdata = pdata >= 18000 ? 36000 - pdata : -pdata;
            }

            // 计算最终角度
            return pdata / 100.0;
        }
        /// <summary>
        /// 计算云台水平/垂直位置指令数据
        /// </summary>
        /// <param name="direction">方向</param>
        /// <param name="angle">角度。水平（0~360），垂直（-90~90）</param>
        /// <returns></returns>
        public static (byte data1, byte data2) AngleToCmdData(EnumOrientation direction, double angle)
        {
            int value;
            if (direction == EnumOrientation.Horizontal)
            {
                angle = angle % 360;
                if (angle < 0)
                    angle += 360;

                // 水平方向计算
                value = (int)(angle * 100);
            }
            else
            {
                if(angle <-90) 
                    angle = -90;
                if(angle > 90) 
                    angle = 90;

                // 垂直方向计算
                if (angle < 0)
                {
                    value = (int)(-angle * 100);
                }
                else
                {
                    value = 36000 - (int)(angle * 100);
                }
            }

            // 计算 DATA1 和 DATA2
            byte data1 = (byte)(value >> 8);
            byte data2 = (byte)(value & 0xFF);

            return (data1, data2);
        }

        private static bool TryParseHexByte(string hex, out byte result)
        {
            return byte.TryParse(
                hex,
                NumberStyles.HexNumber,
                CultureInfo.InvariantCulture,
                out result
            );
        }
    }
}
